using System;
using UnityEngine;

namespace GGPhysics
{
	[Serializable]
	public class PhysicsWorld
	{
		[Serializable]
		public class PhysicsConstants
		{
			public float airDensity = 1.2f;

			public Vector3 gravity = new Vector3(0f, -9.81f, 0f);

			public PhysicsBodyDefinition floorPhysicsDefinitions = new PhysicsBodyDefinition();
		}

		public delegate void OnCollisionDelegate(ref Collision collision);

		protected struct CollisionsAndContacts
		{
			public GGBetterList<Collision> collisions;

			public GGBetterList<Contact> contacts;
		}

		public struct CollisionsAndContactsSettings
		{
			public float deltaTime;

			public float distanceEpsilon;

			public float integrationDelta;
		}

		public PhysicsConstants worldConstants = new PhysicsConstants();

		protected SweepBroadPhase broadPhase = new SweepBroadPhase();

		public bool useBroadPhase;

		public bool useForceResolution;

		public bool exitOnFirstChanceInForceResolution;

		public int totalStepsInForceResolution = 20;

		[NonSerialized]
		public GGBetterList<PhysicsBody> physicsBodies = new GGBetterList<PhysicsBody>();

		public int maxCollisionSubsteps;

		public float minDistanceForContact = 0.001f;

		public float breakStiffnessPower = 1f;

		private GGBetterList<Collision> collisionGroups = new GGBetterList<Collision>();

		private const float epsilon = 1E-06f;

		private GGBetterList<Collision> collisions = new GGBetterList<Collision>();

		public GGBetterList<Collision> sphereCastCollisions = new GGBetterList<Collision>();

		private GGBetterList<Contact> contacts = new GGBetterList<Contact>();

		private SweepBroadPhase overlapBroadPhase = new SweepBroadPhase();

		private GGBetterList<PhysicsBody> dynamicBodies = new GGBetterList<PhysicsBody>();

		public PhysicsBody sphereCastBody { get; protected set; }

		public event OnCollisionDelegate onCollision;

		public event OnCollisionDelegate onAfterCollision;

		public PhysicsWorld()
		{
			sphereCastBody = new PhysicsBody();
			sphereCastBody.type = PhysicsBody.PhysicsBodyType.DynamicBody;
			sphereCastBody.shape = new SphereShape();
			sphereCastBody.physicsDefinitions = new PhysicsBodyDefinition();
		}

		public void Step(float symDeltaTime, float integrateDeltaTime)
		{
			float num = symDeltaTime;
			while (true)
			{
				if (num <= 0f)
				{
					return;
				}
				CollisionsAndContacts collisionsAndContacts = DetectCollisionsAndContactsBroadPhase(num, integrateDeltaTime, minDistanceForContact);
				GGBetterList<Collision> gGBetterList = collisionsAndContacts.collisions;
				if (gGBetterList.Count > 0)
				{
					int num2 = 0;
					bool flag = false;
					while (true)
					{
						for (int i = 0; i < collisionsAndContacts.collisions.Count; i++)
						{
							Collision collision = collisionsAndContacts.collisions[i];
							PhysicsBody body = collision.body1.body;
							PhysicsBody body2 = collision.body2.body;
							ColliderBody body3 = new ColliderBody(body, body.TransformAfterDelta(integrateDeltaTime));
							ColliderBody body4 = new ColliderBody(body2, body2.TransformAfterDelta(integrateDeltaTime));
							Collision collision2;
							flag = CollisionDetection.TryGetCollision(body3, body4, out collision2);
							if (flag)
							{
								break;
							}
						}
						if (flag)
						{
							break;
						}
						for (int j = 0; j < physicsBodies.Count; j++)
						{
							PhysicsBody physicsBody = physicsBodies[j];
							if (physicsBody.shape.isActive && physicsBody.type != 0)
							{
								physicsBody.AdvanceTransform(integrateDeltaTime);
								physicsBody.AdvanceMotion(integrateDeltaTime, worldConstants);
							}
						}
						if (num <= (float)num2 * integrateDeltaTime)
						{
							break;
						}
						num2++;
					}
					if (!flag)
					{
						break;
					}
					Step(integrateDeltaTime);
					num -= (float)(num2 + 1) * integrateDeltaTime;
					continue;
				}
				for (int k = 0; k < physicsBodies.Count; k++)
				{
					PhysicsBody physicsBody2 = physicsBodies[k];
					if (physicsBody2.shape.isActive && physicsBody2.type != 0)
					{
						physicsBody2.AdvanceTransformAndMotion(num, integrateDeltaTime, worldConstants);
					}
				}
				num = 0f;
			}
			num = 0f;
		}

		public void Step(float deltaTime)
		{
			int num = 0;
			float num2 = deltaTime;
			bool flag;
			do
			{
				num++;
				CollisionsAndContacts collisionsAndContacts = DetectCollisionsAndContactsBroadPhase(num2, minDistanceForContact);
				GGBetterList<Collision> gGBetterList = collisionsAndContacts.collisions;
				Collision firstCollision = GetFirstCollision(gGBetterList);
				GGBetterList<Collision> collisionGroup = GetCollisionGroup(gGBetterList, firstCollision);
				flag = num > maxCollisionSubsteps || !firstCollision.isColliding;
				float num3 = num2;
				if (flag)
				{
					num3 = num2;
				}
				else
				{
					num3 = num2 * MathEx.Max(firstCollision.timeOfImpact, 0f);
					num2 = MathEx.Max(0f, num2 - num3);
				}
				for (int i = 0; i < physicsBodies.Count; i++)
				{
					PhysicsBody physicsBody = physicsBodies[i];
					if (!physicsBody.shape.isActive || physicsBody.type == PhysicsBody.PhysicsBodyType.StaticBody)
					{
						continue;
					}
					bool flag2 = false;
					for (int j = 0; j < collisionGroup.Count; j++)
					{
						if (collisionGroup[j].ContainsBody(physicsBody))
						{
							flag2 = true;
							break;
						}
					}
					if (!flag2)
					{
						physicsBody.AdvanceTransform(num3);
						physicsBody.AdvanceMotion(num3, worldConstants);
					}
				}
				if (useForceResolution && firstCollision.isColliding && firstCollision.body1.body.type == PhysicsBody.PhysicsBodyType.DynamicBody && firstCollision.body2.body.type == PhysicsBody.PhysicsBodyType.DynamicBody && (ContactCount(collisionsAndContacts.contacts, firstCollision.body1.body) > 1 || ContactCount(collisionsAndContacts.contacts, firstCollision.body2.body) > 1))
				{
					ResolveCollisionGroupForce(collisionGroup);
				}
				else if (flag)
				{
					ResolveCollisions(gGBetterList);
				}
				else if (firstCollision.isColliding)
				{
					ResolveCollision(firstCollision);
				}
				num++;
			}
			while (!flag && num2 != 0f);
		}

		private int ContactCount(GGBetterList<Contact> contacts, PhysicsBody body)
		{
			int num = 0;
			for (int i = 0; i < contacts.Count; i++)
			{
				Contact contact = contacts[i];
				if (contact.body1.body == body || contact.body2.body == body)
				{
					num++;
				}
			}
			return num;
		}

		private GGBetterList<Collision> GetCollisionGroup(GGBetterList<Collision> collisions, Collision firstCollision)
		{
			collisionGroups.Clear();
			if (!firstCollision.isColliding)
			{
				return collisionGroups;
			}
			collisionGroups.Add(firstCollision);
			return collisionGroups;
		}

		protected Collision GetFirstCollision(GGBetterList<Collision> collisions)
		{
			Collision result = default(Collision);
			for (int i = 0; i < collisions.Count; i++)
			{
				Collision collision = collisions[i];
				if (!result.isColliding || result.timeOfImpact > collision.timeOfImpact)
				{
					result = collision;
				}
			}
			return result;
		}

		protected void Integrate(PhysicsBody body, float deltaTime)
		{
			body.AdvanceMotion(deltaTime, worldConstants);
			body.AdvanceTransform(deltaTime);
		}

		private GGBetterList<Collision> DetectCollisions(float deltaTime)
		{
			return DetectCollisionsBroadPhase(deltaTime);
		}

		private GGBetterList<Collision> DetectCollisionsBroadPhase(float deltaTime)
		{
			collisions.Clear();
			broadPhase.FillPotentialCollisions(physicsBodies, deltaTime, 0f, worldConstants);
			GGBetterList<SweepBroadPhase.CollisionPair> potentialCollisions = broadPhase.potentialCollisions;
			Collision collision = default(Collision);
			for (int i = 0; i < potentialCollisions.Count; i++)
			{
				SweepBroadPhase.CollisionPair collisionPair = potentialCollisions[i];
				Collision collision2;
				if ((collisionPair.body1.body.type != 0 || collisionPair.body2.body.type != 0) && CollisionDetection.TryGetCollision(collisionPair.body1, collisionPair.body2, out collision2))
				{
					collisions.Add(collision2);
					if (!collision.isColliding || collision.timeOfImpact > collision2.timeOfImpact)
					{
						collision = collision2;
					}
				}
			}
			if (collision.isColliding)
			{
			}
			return collisions;
		}

		private CollisionsAndContacts DetectCollisionsAndContactsBroadPhase(float deltaTime, float distanceEpsilon)
		{
			CollisionsAndContactsSettings s = default(CollisionsAndContactsSettings);
			s.deltaTime = deltaTime;
			s.distanceEpsilon = distanceEpsilon;
			s.integrationDelta = 0f;
			return DetectCollisionsAndContactsBroadPhase(s);
		}

		private CollisionsAndContacts DetectCollisionsAndContactsBroadPhase(float deltaTime, float integrateTime, float distanceEpsilon)
		{
			CollisionsAndContactsSettings s = default(CollisionsAndContactsSettings);
			s.deltaTime = deltaTime;
			s.distanceEpsilon = distanceEpsilon;
			s.integrationDelta = integrateTime;
			return DetectCollisionsAndContactsBroadPhase(s);
		}

		private CollisionsAndContacts DetectCollisionsAndContactsBroadPhase(CollisionsAndContactsSettings s)
		{
			float deltaTime = s.deltaTime;
			float distanceEpsilon = s.distanceEpsilon;
			collisions.Clear();
			contacts.Clear();
			broadPhase.FillPotentialCollisions(physicsBodies, deltaTime, s.integrationDelta, worldConstants);
			GGBetterList<SweepBroadPhase.CollisionPair> potentialCollisions = broadPhase.potentialCollisions;
			Collision collision = default(Collision);
			for (int i = 0; i < potentialCollisions.Count; i++)
			{
				SweepBroadPhase.CollisionPair collisionPair = potentialCollisions[i];
				if (collisionPair.body1.body.type == PhysicsBody.PhysicsBodyType.StaticBody && collisionPair.body2.body.type == PhysicsBody.PhysicsBodyType.StaticBody)
				{
					continue;
				}
				Collision collision2;
				if (CollisionDetection.TryGetCollision(collisionPair.body1, collisionPair.body2, out collision2))
				{
					collisions.Add(collision2);
					if (!collision.isColliding || collision.timeOfImpact > collision2.timeOfImpact)
					{
						collision = collision2;
					}
				}
				if (collisionPair.body1.body.type == PhysicsBody.PhysicsBodyType.DynamicBody && collisionPair.body2.body.type == PhysicsBody.PhysicsBodyType.DynamicBody)
				{
					Contact item = default(Contact);
					PhysicsBody body = collisionPair.body1.body;
					PhysicsBody body2 = collisionPair.body2.body;
					item.body1.body = collisionPair.body1.body;
					item.body2.body = collisionPair.body2.body;
					float sqrMagnitude = (body.currentTransform.position - body2.currentTransform.position).OnGround().sqrMagnitude;
					float distanceToSurface = body.shape.GetDistanceToSurface(Vector3.down);
					float distanceToSurface2 = body.shape.GetDistanceToSurface(Vector3.down);
					float num = distanceToSurface + distanceToSurface2 + distanceEpsilon;
					if (sqrMagnitude < num * num)
					{
						contacts.Add(item);
					}
				}
			}
			CollisionsAndContacts result = default(CollisionsAndContacts);
			result.collisions = collisions;
			result.contacts = contacts;
			return result;
		}

		public GGBetterList<Collision> SphereCastAll(Ray ray, float radius, float mass, float maxDistance, PhysicsBodyDefinition definitions, Vector3 angVel)
		{
			sphereCastBody.physicsDefinitions = definitions;
			GGBetterList<Collision> gGBetterList = sphereCastCollisions;
			SphereShape shape = sphereCastBody.GetShape<SphereShape>();
			shape.radius = radius;
			shape.mass = mass;
			sphereCastBody.currentTransform.position = ray.origin;
			PhysicsBody.Transform nextTransform = default(PhysicsBody.Transform);
			nextTransform.Set(sphereCastBody.currentTransform);
			nextTransform.position = ray.origin + ray.direction.normalized * maxDistance;
			ColliderBody body = new ColliderBody(sphereCastBody, nextTransform);
			sphereCastBody.myMotion.velocity = ray.direction.normalized * maxDistance;
			sphereCastBody.myMotion.angularVelocity = angVel;
			gGBetterList.Clear();
			CollisionDetection.AABBBounds bounds = body.bounds;
			for (int i = 0; i < physicsBodies.Count; i++)
			{
				PhysicsBody physicsBody = physicsBodies[i];
				if (physicsBody.shape.isActive)
				{
					CollisionDetection.AABBBounds bounds2 = physicsBody.shape.GetBounds(physicsBody.currentTransform, physicsBody.currentTransform);
					Collision collision;
					if (CollisionDetection.AABBBounds.CanCollide(ref bounds2, ref bounds) && CollisionDetection.TryGetCollision(body, new ColliderBody(physicsBody, physicsBody.currentTransform), out collision))
					{
						gGBetterList.Add(collision);
					}
				}
			}
			return gGBetterList;
		}

		private int BodyCountInGroup(GGBetterList<Collision> colGroup, PhysicsBody body)
		{
			int num = 0;
			for (int i = 0; i < colGroup.Count; i++)
			{
				Collision collision = colGroup[i];
				if (!collision.isCanceled && (collision.collidingBody1.type != 0 || collision.collidingBody2.type != 0) && collision.ContainsBody(body))
				{
					num++;
				}
			}
			return num;
		}

		private GGBetterList<Contact> DetectContactsBroadPhase(GGBetterList<PhysicsBody> bodies, float dt)
		{
			contacts.Clear();
			overlapBroadPhase.FillPotentialCollisions(bodies, dt, 0f, worldConstants);
			GGBetterList<SweepBroadPhase.CollisionPair> potentialCollisions = overlapBroadPhase.potentialCollisions;
			Contact item = default(Contact);
			for (int i = 0; i < potentialCollisions.Count; i++)
			{
				SweepBroadPhase.CollisionPair collisionPair = potentialCollisions[i];
				if (collisionPair.body1.body.type != 0 || collisionPair.body2.body.type != 0)
				{
					PhysicsBody body = collisionPair.body1.body;
					PhysicsBody body2 = collisionPair.body2.body;
					item.body1.body = collisionPair.body1.body;
					item.body2.body = collisionPair.body2.body;
					float sqrMagnitude = (body.currentTransform.position - body2.currentTransform.position).OnGround().sqrMagnitude;
					float distanceToSurface = body.shape.GetDistanceToSurface(Vector3.down);
					float distanceToSurface2 = body.shape.GetDistanceToSurface(Vector3.down);
					float num = distanceToSurface + distanceToSurface2;
					if (sqrMagnitude < num * num)
					{
						contacts.Add(item);
					}
				}
			}
			return contacts;
		}

		private void ResolveCollisionGroupForce(GGBetterList<Collision> colGroup)
		{
			if (colGroup.Count == 0)
			{
				return;
			}
			Collision collision = colGroup[0];
			if (!collision.isColliding)
			{
				return;
			}
			if (this.onCollision != null)
			{
				this.onCollision(ref collision);
			}
			if (collision.isCanceled)
			{
				return;
			}
			float num = 5.84E+09f;
			float mass = collision.collidingBody1.shape.mass;
			float distanceToSurface = collision.collidingBody1.shape.GetDistanceToSurface(Vector3.down);
			float num2 = 0.34f;
			float num3 = mass * mass / (mass + mass);
			float num4 = 8f * num * Mathf.Sqrt(distanceToSurface / 2f) / (15f * (1f - num2 * num2));
			float magnitude = (collision.body1.motionBeforeCollision.velocity - collision.body2.motionBeforeCollision.velocity).magnitude;
			float num5 = 2.94f * Mathf.Pow(num3 * num3 / (num4 * num4 * magnitude), 0.2f);
			float num6 = num5 / (float)totalStepsInForceResolution;
			for (int i = 0; i < colGroup.Count; i++)
			{
				Collision value = colGroup[i];
				if (value.collidingBody1.type != 0 || value.collidingBody2.type != 0)
				{
					value.body1.body.currentTransform = value.body1.transformAtCollision;
					value.body2.body.currentTransform = value.body2.transformAtCollision;
				}
				colGroup[i] = value;
			}
			float num7 = 0f;
			dynamicBodies.Clear();
			for (int j = 0; j < physicsBodies.Count; j++)
			{
				PhysicsBody physicsBody = physicsBodies[j];
				if (physicsBody.type == PhysicsBody.PhysicsBodyType.DynamicBody)
				{
					dynamicBodies.Add(physicsBody);
					num7 = MathEx.Max(physicsBody.myMotion.velocity.magnitude, num7);
				}
			}
			float num8 = num7 * num7;
			overlapBroadPhase.Clear();
			float num9 = 0f;
			for (int k = 0; k < totalStepsInForceResolution; k++)
			{
				for (int l = 0; l < dynamicBodies.Count; l++)
				{
					PhysicsBody physicsBody2 = dynamicBodies[l];
					physicsBody2.AdvanceTransform(num6);
				}
				GGBetterList<Contact> gGBetterList = DetectContactsBroadPhase(dynamicBodies, num6);
				if (gGBetterList.Count == 0)
				{
					if (exitOnFirstChanceInForceResolution && k > 4)
					{
						break;
					}
					continue;
				}
				for (int m = 0; m < gGBetterList.Count; m++)
				{
					Contact contact = gGBetterList[m];
					PhysicsBody body = contact.body1.body;
					PhysicsBody body2 = contact.body2.body;
					Vector3 position = body.currentTransform.position;
					Vector3 position2 = body2.currentTransform.position;
					float distanceToSurface2 = body.shape.GetDistanceToSurface(Vector3.down);
					float distanceToSurface3 = body2.shape.GetDistanceToSurface(Vector3.down);
					float mass2 = body.shape.mass;
					float mass3 = body2.shape.mass;
					Vector3 vector = (position - position2).OnGround();
					float magnitude2 = vector.magnitude;
					Vector3 vector2 = vector / magnitude2;
					float num10 = distanceToSurface2 + distanceToSurface3 - magnitude2;
					if (!(num10 < 0f))
					{
						float num11 = num4 * num10;
						if (num10 > 1f)
						{
							num11 = num4 * Mathf.Pow(num10, breakStiffnessPower);
						}
						float num12 = MathEx.Min(num11 / mass2 * num6, num7);
						float num13 = MathEx.Min(num11 / mass3 * num6, num7);
						body.myMotion.velocity += num12 * vector2;
						body2.myMotion.velocity -= num13 * vector2;
						if (body.myMotion.velocity.sqrMagnitude > num8)
						{
							body.myMotion.velocity = vector2 * num7;
						}
						if (body2.myMotion.velocity.sqrMagnitude > num8)
						{
							body2.myMotion.velocity = -vector2 * num7;
						}
					}
				}
				num9 += num6;
			}
		}

		private void ResolveCollisionGroup(GGBetterList<Collision> colGroup)
		{
			for (int i = 0; i < colGroup.Count; i++)
			{
				Collision collision = colGroup[i];
				if ((collision.collidingBody1.type != 0 || collision.collidingBody2.type != 0) && this.onCollision != null)
				{
					this.onCollision(ref collision);
				}
				if (!collision.isCanceled && (collision.collidingBody1.type != 0 || collision.collidingBody2.type != 0))
				{
					collision.body1.useMassMult = true;
					collision.body1.massMult = 1f / (float)BodyCountInGroup(colGroup, collision.collidingBody1);
					collision.body2.useMassMult = true;
					collision.body2.massMult = 1f / (float)BodyCountInGroup(colGroup, collision.collidingBody2);
					collision.ResolveCollision();
				}
				colGroup[i] = collision;
			}
			for (int j = 0; j < colGroup.Count; j++)
			{
				Collision collision2 = colGroup[j];
				collision2.collidingBody1.myMotion.Stop();
				collision2.collidingBody2.myMotion.Stop();
			}
			for (int k = 0; k < colGroup.Count; k++)
			{
				Collision collision3 = colGroup[k];
				if (!collision3.isCanceled && (collision3.collidingBody1.type != 0 || collision3.collidingBody2.type != 0))
				{
					int num = BodyCountInGroup(colGroup, collision3.collidingBody1);
					int num2 = BodyCountInGroup(colGroup, collision3.collidingBody2);
					collision3.collidingBody1.myMotion.velocity += collision3.body1.motionResolution.velocity / num;
					collision3.collidingBody1.myMotion.angularVelocity += collision3.body1.motionResolution.angularVelocity / num;
					collision3.collidingBody2.myMotion.velocity += collision3.body2.motionResolution.velocity / num2;
					collision3.collidingBody2.myMotion.angularVelocity += collision3.body2.motionResolution.angularVelocity / num2;
				}
			}
		}

		private void ResolveCollision(Collision col)
		{
			if ((col.collidingBody1.type != 0 || col.collidingBody2.type != 0) && this.onCollision != null)
			{
				this.onCollision(ref col);
			}
			if (!col.isCanceled)
			{
				if (col.collidingBody1.type != 0 || col.collidingBody2.type != 0)
				{
					col.ResolveCollisionAndApply();
				}
				if (this.onAfterCollision != null)
				{
					this.onAfterCollision(ref col);
				}
			}
		}

		private void ResolveCollisions(GGBetterList<Collision> collisions)
		{
			for (int i = 0; i < collisions.Count; i++)
			{
				Collision col = collisions[i];
				ResolveCollision(col);
			}
		}

		public void AddPhysicsBody(PhysicsBody body)
		{
			if (body != null && !ContainsBody(body))
			{
				physicsBodies.Add(body);
				if (broadPhase != null)
				{
					broadPhase.Clear();
				}
			}
		}

		public void ClearAllBodies()
		{
			physicsBodies.Clear();
			broadPhase.Clear();
		}

		public void RemovePhysicsBody(PhysicsBody body)
		{
			if (body != null)
			{
				physicsBodies.Remove(body);
				if (broadPhase != null)
				{
					broadPhase.Clear();
				}
			}
		}

		public bool ContainsBody(PhysicsBody body)
		{
			if (body == null)
			{
				return false;
			}
			if (!physicsBodies.Contains(body))
			{
				return false;
			}
			return true;
		}
	}
}
